/* 
//工程：自平衡自行车
// 作者：大玉菌
// 日期：240828
// 警告：请勿用于商业用途
*/
#include <Arduino.h>
#include "Wire.h" 
#include "I2Cdev.h"
#include "MPU6050.h"
#include <ESP32Encoder.h>
#include <PS4Controller.h>
#include <EEPROM.h>

MPU6050 accelgyro;                                           //IMU
ESP32Encoder encoder;                                        //编码器

/*GPIO*/
#define DEBUG         1                                      //调试模式
#define PWM_1         5                                      //动量轮电机PWM脉冲信号
#define DIR_1         17                                     //动量轮电机方向信号  
#define PWM_2         26                                     //移动电机PWM脉冲信号  
#define DIR_2         27                                     //移动电机方向信号 
#define BRAKE         25                                     //刹车
#define VBAT          12                                     //电量读取 
#define LED           32                                     //指示灯

#define ENC_1         4                                      //编码器信号A
#define ENC_2         16                                     //编码器信号B  

#define I2C_MASTER_SCL_IO 22                                 /*!< gpio number for I2C master clock */
#define I2C_MASTER_SDA_IO 21                                 /*!< gpio number for I2C master data  */

#define SERVOPIN        23                                   //舵机端口


#define STEERING_MAX    20    
#define SPEED_MAX       80
#define STEERING_CENTER 87                                    //理论上是90 但是安装时候有偏差 请自行校准
#define ST_LIMIT        5
#define SPEED_LIMIT     4

/*PWM channel*/
const int PWM1_CHL = 0;                                      //动量轮                              
const int PWM2_CHL = 1;                                      //后轮
const int Servo_CHL = 8;                                     //舵机

/*Variable*/
long enc_count;                                              //编码器计数器
float alpha = 0.4;                                       

// float offset= -0.7;                                       //角度偏置 白色
// //LQR 系数
// float K1 = 111.6;
// float K2 = 9.4;
// float K3 = 9.5;
// float K4 = 0.6;

float offset= -0.2;                                          //角度偏置 黄色
//LQR 系数
float K1 = 94.5;                                            //MATLAB生成
float K2 = 8;                                              //MATLAB生成 
float K3 = 9.4;                                             
float K4 = 1.0;

//实际调参后的参数
//K = 92.5 7.6 10.3 1.0

uint K1_E2P;
uint K2_E2P;
uint K3_E2P;
uint K4_E2P;

float loop_time = 10;                                         //微分步长10ms

bool vertical = false;                                        //直立标志位
bool calibrating = false;                                     //电量标志位

int16_t motor_speed;                                          //反作用轮实际速度                    
int32_t motor_pos;                                            //反作用轮位置
                  
int16_t ax, ay, az, gx, gy, gz;                               //加速度计陀螺仪原始数据
float aax=0, aay=0, aaz=0, agx=0, agy=0, agz=0,aax1;          //角度变量
long axo = 0, ayo = 0, azo = 0;                               //加速度计偏移量
long gxo = 0, gyo = 0, gzo = 0;                               //陀螺仪偏移量

float pi = 3.1415926;
float AcceRatio = 16384.0;                                    //加速度计比例系数  (32768.0/2)
float GyroRatio = 131.0;                                      //陀螺仪比例系数  (32768.0/250)

uint8_t n_sample = 8;                                         //加速度计滤波算法采样个数
float aaxs[8] = {0}, aays[8] = {0};                           //x,y轴采样队列
long aax_sum, aay_sum;                                        //x,y轴采样和

float a_x[10]={0}, a_y[10]={0} ,g_x[10]={0} ,g_y[10]={0};     //加速度计协方差计算队列
float Px=1, Rx, Kx, Sx, Vx, Qx;                               //x轴卡尔曼变量
float Py=1, Ry, Ky, Sy, Vy, Qy;                               //y轴卡尔曼变量
float euler_angle;                                            //euler_angle:小车倾角
float gyroX;                                                  //角速度
float gyroXfilter;                                            //滤波后的角速度                                               
int counter=0,counter_led=0; 
long currentT, previousT_1, previousT_2 = 0,previousT_3;      //计数器
int steering_remote = 0, speed_remote = 0, speed_value = 0, steering_value = STEERING_CENTER;

void Motor1_control(int sp);
void Motor2_control(int sp);
void Kalman_filter();
int calculatePWM(int degree);
uint ReadKValueFromE2p();
void WriteKValueToE2p();

void setup()
{
  delay(1000);
  Serial.begin(115200);
  PS4.begin("74:29:af:b7:ef:32");                             //PS4手柄 MAC地址    
  Serial.println(F("Start Balance Bike..."));
  pinMode(DIR_1, OUTPUT);  
  pinMode(DIR_2, OUTPUT);
  pinMode(BRAKE, OUTPUT);
  pinMode(LED, OUTPUT);
  pinMode(ENC_1, INPUT);
  pinMode(ENC_2, INPUT);
  pinMode(VBAT, INPUT);
  
  ledcSetup(PWM1_CHL, 10000, 8);
  ledcAttachPin(PWM_1, PWM1_CHL);
  ledcSetup(PWM2_CHL, 10000, 8);
  ledcAttachPin(PWM_2, PWM2_CHL);

  ledcSetup(Servo_CHL, 50, 8);                                
  ledcAttachPin(SERVOPIN, Servo_CHL);

  Motor1_control(0);
  Motor2_control(0);

  ESP32Encoder::useInternalWeakPullResistors = puType::up;    // Enable the weak pull up resistors
  encoder.attachHalfQuad(ENC_1, ENC_2);                       // Attache pins for use as encoder pins
  encoder.setCount(0);                                        // set starting count value after attaching

  /*Initial MPU */
  Wire.begin(I2C_MASTER_SDA_IO,I2C_MASTER_SCL_IO);
  accelgyro.initialize();
  if (accelgyro.testConnection()) Serial.println(F("MPU6050 OK"));
    else Serial.println(F("MPU6050 fail"));   

  /*Initial Kalman Filter*/  
  unsigned short times = 200;                                 //采样数
  for(int i=0;i<times;i++)                                    //获取MPU6050水平时各轴的偏移量 
  {         
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);       //read raw data from mpu
    axo += ax; ayo += ay; azo += az;      
    gxo += gx; gyo += gy; gzo += gz;
  }
  axo /= times; ayo /= times; azo /= times;                   //calculate offset
  gxo /= times; gyo /= times; gzo /= times;

  ledcWrite(Servo_CHL, calculatePWM(STEERING_CENTER));
  delay(1000);

  if(ReadKValueFromE2p() == 0)
  {
    Serial.println(F("Use default K value"));
  }  
}

void loop() {
  
  currentT = millis();
  if (currentT - previousT_1 >= loop_time) {
    previousT_1 = currentT;

    Kalman_filter();
    euler_angle=-agx+offset;  
    if(abs(euler_angle) > 10)   
      vertical = false;
    if(abs(euler_angle) < 0.4)   
      vertical = true;
    #ifdef DEBUG
      if(vertical)
    #else
      if(vertical &&  calibrating)
    #endif
    {
        digitalWrite(BRAKE, HIGH); 
        enc_count=encoder.getCount();       
        motor_speed=enc_count;
        encoder.setCount(0);
        gyroXfilter = alpha * gyroX + (1 - alpha) * gyroXfilter;      //一阶互补滤波后的角速度
        motor_pos += motor_speed;
        motor_pos = constrain(motor_pos, -110, 110);                  //110 ticks per rev

        int lqr = constrain(K1 * euler_angle + K2 * gyroXfilter - K3 * motor_speed - K4 * motor_pos, -255, 255); //Linear–quadratic regulator
        Motor1_control(-lqr);
        
        if ((speed_value - speed_remote) > SPEED_LIMIT)
          speed_value -= SPEED_LIMIT;
        else if ((speed_value - speed_remote) < -SPEED_LIMIT)
          speed_value += SPEED_LIMIT;
        else
          speed_value = speed_remote; 
      
      Motor2_control(speed_value);

      if (counter>25){
        #ifdef DEBUG
          Serial.print("AGX: ");
          Serial.print(euler_angle);Serial.print(",");
          Serial.print(gyroXfilter);Serial.print(",");
          Serial.print(motor_pos);Serial.print(",");
          Serial.print(motor_speed);Serial.print(",");
          Serial.print(-lqr);Serial.print(",");Serial.print(offset);
          Serial.println();
         #endif
        if ((steering_value - STEERING_CENTER - steering_remote) > ST_LIMIT)
          steering_value -= ST_LIMIT;
        else if ((steering_value - STEERING_CENTER - steering_remote) < -ST_LIMIT)
          steering_value += ST_LIMIT;
        else
          steering_value = STEERING_CENTER + steering_remote;
        ledcWrite(Servo_CHL, calculatePWM(steering_value));
        counter=0;
      }
      counter++;
    }
    else
    {
      digitalWrite(BRAKE, LOW); 
      Motor1_control(0);
      Motor2_control(0);  
    }
    if(!calibrating)
    {
      if(counter_led == 50){
        digitalWrite(LED , LOW);
      }
      if(counter_led == 100){
        digitalWrite(LED , HIGH);
        counter_led = 0;
      }
      counter_led ++ ;
    }
  }
  if (currentT - previousT_2 >= 2000) {    
      float vol = analogRead(VBAT) * 3.357 / 4096 * 11 + 0.8;  
      #ifdef DEBUG  
      Serial.print("Vol: ");
      Serial.println(vol);
      #endif
      if(vol < 10.8) 
          calibrating = false; 
      else
          calibrating = true;     
     previousT_2 = currentT;
   } 
   if (currentT - previousT_3 >= 300) {    
    if (PS4.isConnected()) {
      if (PS4.Up()) {
        K1 += 1; 
        Serial.println("Up Button");
      }
      if (PS4.Left()){
        K1 -= 1;   
        Serial.println("Left Button");  
      } 
      if (PS4.Right())
      {
        Serial.println("Right Button");
        K2 += 0.2;     
      } 
      if (PS4.Down()){
        Serial.println("Down Button");  
        K2 -= 0.2; 
      } 
      if (PS4.Triangle()) 
      {
        Serial.println("Triangle Button");
        K3 += 0.2;
      }
      if (PS4.Cross())
      {
        Serial.println("Cross Button");   
        K3 -= 0.2;
      } 
      if (PS4.Square()) 
      {
        Serial.println("Square Button");
        K4 += 0.05;
      }
      if (PS4.Circle()) 
      {
        Serial.println("Circle Button");  
        K4 -= 0.05;
      }

      // if (PS4.UpRight()) Serial.println("Up Right");
      // if (PS4.DownRight()) Serial.println("Down Right");
      // if (PS4.UpLeft()) Serial.println("Up Left");
      // if (PS4.DownLeft()) Serial.println("Down Left");

      // if (PS4.L1()) Serial.println("L1 Button");
      // if (PS4.R1()) Serial.println("R1 Button");

      // if (PS4.Share()) Serial.println("Share Button");
      if (PS4.Options()){
        Serial.println("Options Button");   
        WriteKValueToE2p();
      } 
      // if (PS4.L3()) Serial.println("L3 Button");
      // if (PS4.R3()) Serial.println("R3 Button");

      // if (PS4.PSButton()) Serial.println("PS Button");
      // if (PS4.Touchpad()) Serial.println("Touch Pad Button");

      // if (PS4.L2()) {
      //   Serial.printf("L2 button at %d\n", PS4.L2Value());
      // }
      // if (PS4.R2()) {
      //   Serial.printf("R2 button at %d\n", PS4.R2Value());
      // }

      if (PS4.LStickX()) {
//        Serial.printf("Left Stick x at %d\n", PS4.LStickX());
      }
      if (PS4.LStickY()) {
//        Serial.printf("Left Stick y at %d\n", PS4.LStickY());
        speed_remote = PS4.LStickY()/4;
      }
      if (PS4.RStickX()) {
//       Serial.printf("Right Stick x at %d\n", PS4.RStickX());

      }
      if (PS4.RStickY()) {
          steering_remote = PS4.RStickY()/10;
//        Serial.printf("Right Stick y at %d\n", PS4.RStickY());
      }

      // if (PS4.Charging()) Serial.println("The controller is charging");
      // if (PS4.Audio()) Serial.println("The controller has headphones attached");
      // if (PS4.Mic()) Serial.println("The controller has a mic attached");

//      Serial.printf("Battery Level : %d\n", PS4.Battery());

 //     Serial.println();
    }
    previousT_3 = currentT;
   }  
}

uint ReadKValueFromE2p()
{
  int i;
  byte k[16];
  byte valuefromRead;
  valuefromRead = EEPROM.read(0);
  if(valuefromRead != 0x55)
  {
    return 0;
  }
  for(i=1;i<17;i++)
    k[i - 1] = EEPROM.read(i);
  K1_E2P =  (k[0]<<24) + (k[1]<<16) + (k[2]<<8) + k[3];
  K2_E2P =  (k[4]<<24) + (k[5]<<16) + (k[6]<<8) + k[7];
  K3_E2P =  (k[8]<<24) + (k[9]<<16) + (k[10]<<8) + k[11];
  K4_E2P =  (k[12]<<24) + (k[13]<<16) + (k[14]<<8) + k[15];
  K1 = (float)(K1_E2P/100.0);
  K2 = (float)(K2_E2P/100.0);
  K3 = (float)(K3_E2P/100.0);
  K4 = (float)(K4_E2P/100.0);
  Serial.printf("K = %f, %f, %f, %f",K1,K2,K3,K4);
  return 1;
}

void WriteKValueToE2p()
{
  int i;
  byte k[17];
  k[0] = 0x55;
  K1_E2P = (uint)(K1*100);
  k[1] = (byte)((K1_E2P>>24)&0xff);
  k[2] = (byte)((K1_E2P>>16)&0xff);
  k[3] = (byte)((K1_E2P>>8)&0xff);
  k[4] = (byte)(K1_E2P&0xff);
  K2_E2P = (uint)(K2*100);
  k[5] = (byte)((K2_E2P>>24)&0xff);
  k[6] = (byte)((K2_E2P>>16)&0xff);
  k[7] = (byte)((K2_E2P>>8)&0xff);
  k[8] = (byte)(K2_E2P&0xff);
  K3_E2P = (uint)(K3*100);
  k[9] = (byte)((K3_E2P>>24)&0xff);
  k[10] = (byte)((K3_E2P>>16)&0xff);
  k[11] = (byte)((K3_E2P>>8)&0xff);
  k[12] = (byte)(K3_E2P&0xff);  
  K4_E2P = (uint)(K4*100);
  k[13] = (byte)((K4_E2P>>24)&0xff);
  k[14] = (byte)((K4_E2P>>16)&0xff);
  k[15] = (byte)((K4_E2P>>8)&0xff);
  k[16] = (byte)(K4_E2P&0xff);  
  for(i=0;i<17;i++)
    EEPROM.write(i,k[i]);
  EEPROM.commit();
}

/*函数功能：动量轮电机驱动*/
void Motor1_control(int sp) {
  if (sp > 0) digitalWrite(DIR_1, LOW);
    else digitalWrite(DIR_1, HIGH);
  ledcWrite(PWM1_CHL, 255 - abs(sp));                     //占空比越大，电机转速越小
}

/*函数功能：后轮电机驱动*/
void Motor2_control(int sp) {
  if (sp > 0) digitalWrite(DIR_2, LOW);
    else digitalWrite(DIR_2, HIGH);
  ledcWrite(PWM2_CHL, 255 - abs(sp));
}


/*函数功能：卡尔曼滤波*/
void Kalman_filter(){
   char buffer[50];

  //请参考https://www.kechuang.org/t/89647
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //raw data from MPU

  float accx = ax / AcceRatio;                            //x轴加速度           
  float accy = ay / AcceRatio;                            //y轴加速度
  float accz = az / AcceRatio;                            //z轴加速度

  aax = atan(accy / accz) * (-180) / pi;                  //y轴对于z轴的夹角 弧度转换为度 
  aay = atan(accx / accz) * 180 / pi;                     //x轴对于z轴的夹角
  aaz = atan(accz / accy) * 180 / pi;                     //z轴对于y轴的夹角

  aax_sum = 0;                                            // 对于加速度计原始数据的滑动加权滤波算法        
  aay_sum = 0;
  for(int i=1;i<n_sample;i++){
    aaxs[i-1] = aaxs[i];
    aax_sum += aaxs[i] * i;
    aays[i-1] = aays[i];
    aay_sum += aays[i] * i;
  }
  aaxs[n_sample-1] = aax;
  aax_sum += aax * n_sample;
  aax = (aax_sum / (11*n_sample/2.0)) * 9 / 7.0;          //角度调幅至0-90°
  aays[n_sample-1] = aay;                        
  aay_sum += aay * n_sample;                     
  aay = (aay_sum / (11*n_sample/2.0)) * 9 / 7.0;

  gyroX =(gx-gxo) / GyroRatio;                            //X轴倾角

  float gyrox = - (gx-gxo) / GyroRatio * loop_time/1000;  //x轴角速度
  float gyroy = - (gy-gyo) / GyroRatio * loop_time/1000;  //y轴角速度
  float gyroz = - (gz-gzo) / GyroRatio * loop_time/1000;  //z轴角速度
  agx += gyrox;                                           //x轴角速度积分  
  agy += gyroy;                                           //y轴角速度积分
  agz += gyroz;
  //   sprintf(buffer, "%f,%f,%f", aax, gyroX,agx);
  // Serial.println(buffer);

  /* kalmen start */
  Sx = 0; Rx = 0;
  Sy = 0; Ry = 0;
  for(int i=1;i<10;i++){                                  //测量值平均值运算
    a_x[i-1] = a_x[i];                      
    Sx += a_x[i];
    a_y[i-1] = a_y[i];
    Sy += a_y[i];
  }
  a_x[9] = aax;
  Sx += aax;
  Sx /= 10;                                               //x轴加速度平均值
  a_y[9] = aay;
  Sy += aay;
  Sy /= 10;                                               //y轴加速度平均值

  for(int i=0;i<10;i++){
    Rx += sq(a_x[i] - Sx);
    Ry += sq(a_y[i] - Sy);
  }
  Rx = Rx / 9;                                            //得到方差
  Ry = Ry / 9;                        

  Px = Px + 0.0025;                         
  Kx = Px / (Px + Rx);                                    //计算卡尔曼增益
  agx = agx + Kx * (aax - agx);                           //陀螺仪角度与加速度计速度叠加       
  Px = (1 - Kx) * Px;                                     //更新p值
  Py = Py + 0.0025;
  Ky = Py / (Py + Ry);
  agy = agy + Ky * (aay - agy);
  Py = (1 - Ky) * Py;
  
  /* kalmen end */

}

/*函数功能：定义函数用于输出PWM的占空比*/
int calculatePWM(int degree) 
{ 
  //0-180度
  //20ms周期内，高电平持续时长0.5-2.5ms，对应0-180度舵机角度，参考上面的180度对应图。
  const float deadZone = 6.4;//对应0.5ms（0.5ms/(20ms/256）)
  const float max = 32;//对应2.5ms（2.5ms/（20ms/256））
  if (degree < 0)
    degree = 0;
  if (degree > 180)
    degree = 180;
  return (int)(((max - deadZone) / 180) * degree + deadZone); //返回度数对应的高电平的数值
}

/*
参考资料：https://bbs.makertime.org/thread-2685-1-1.html
*/
